Skip to content

improvement: faithful, faster demo_circle#16

Merged
jimsynz merged 1 commit into
mainfrom
improve-demo-circle
May 14, 2026
Merged

improvement: faithful, faster demo_circle#16
jimsynz merged 1 commit into
mainfrom
improve-demo-circle

Conversation

@jimsynz
Copy link
Copy Markdown
Collaborator

@jimsynz jimsynz commented May 12, 2026

Summary

The demo_circle command previously fired waypoint commands on a fixed 150 ms cadence regardless of whether the arm had reached the previous target, so the EE perpetually lagged one waypoint behind the planned trajectory and the traced path bore little resemblance to a circle. The start position (0.15, 0.0, 0.20) was also genuinely unreachable within the joint limits — three joints had to clamp to their limit, so the start move never converged.

command/demo_circle.ex

  • Move the start to (0.25, 0.0, 0.30), well inside the joint-limited reachable workspace.
  • Replace the fixed Process.sleep/1 between waypoints with wait_for_arrival/4, which polls the position estimator's FK until the EE is within settle_tolerance_m of the commanded target or settle_timeout_ms elapses.
  • Solve IK directly via DLS.solve with the previous IK solution as the warm-start seed, bypassing the position-estimator pollution of robot_state that Motion.move_to is subject to. Adjacent waypoints differ by ~3° in joint space, so this brings IK convergence from ~17 iterations to 1-3 per waypoint.
  • Send the resulting joint positions via BB.Motion.send_positions/3.
  • Tune IK opts for the warm-start case: tolerance: 2e-3, step_size: 0.3, lambda: 0.1, max_iterations: 15.
  • New goal params: settle_tolerance_m (default 5 mm) and settle_timeout_ms (default 1500 ms). Drops delay.

robot.ex

  • Bump every joint's acceleration limit from 720 °/s² to 2160 °/s². Consumed only by BB.Sim.Actuator for its trapezoidal motion profile — speeds up the visual sim without affecting real-hardware behaviour (Feetech STS3215 uses its own internal velocity profile).

Result

End-to-end the sim demo now completes in ~8 s (previously ~25 s with wait_for_arrival at the original acceleration, or producing a non-circular trace at the original 150 ms cadence) and the EE actually visits every commanded point with residuals < 1 mm.

Related

The honest reachability reporting that surfaced the unreachable start position depends on beam-bots/bb_ik_dls#24 (DLS now respects joint limits during iteration). Without that fix the buggy start would silently "succeed" with reached: true. The new start (0.25, 0.0, 0.30) is genuinely reachable on either version, so this PR is not blocked on that one — but the diagnostics path that exposed the bug requires it.

Test plan

  • mix check --no-retry passes locally (5 tests, 0 failures; credo, dialyzer, formatter, reuse, gettext all clean)
  • Verified visually via the LiveView dashboard in sim mode — arm traces a recognisable circle in the XZ plane at radius 30 mm
  • Try on real hardware

The `demo_circle` command previously fired waypoint commands on a fixed
150 ms cadence regardless of whether the arm had reached the previous
target, so the EE perpetually lagged one waypoint behind the planned
trajectory and the traced path bore little resemblance to a circle. The
start position `(0.15, 0.0, 0.20)` was also genuinely unreachable within
the joint limits — three joints had to clamp to their limit, so the start
move never converged.

Changes to `command/demo_circle.ex`:

- Move the start to `(0.25, 0.0, 0.30)`, which is well inside the
  joint-limited reachable workspace and lets the entire 30 mm-radius
  circle stay reachable.
- Replace the fixed `Process.sleep/1` between waypoints with
  `wait_for_arrival/4`, which polls the position estimator's FK until the
  EE is within `settle_tolerance_m` of the commanded target or
  `settle_timeout_ms` elapses.
- Solve IK directly via `DLS.solve` with the **previous IK solution** as
  the warm-start seed instead of relying on `Motion.move_to` (whose
  warm-start gets clobbered by the position estimator during arm motion).
  Adjacent waypoints differ by ~3° in joint space, so this brings IK
  convergence from ~17 iterations to 1-3 per waypoint.
- Send the resulting joint positions via `BBMotion.send_positions/3`
  (bypassing IK on the second call).
- Tune the IK solver for the warm-start case: `tolerance: 2e-3,
  step_size: 0.3, lambda: 0.1, max_iterations: 15`. The tight default
  tolerance (0.1 mm) just wasted iterations when the settle tolerance is
  5 mm anyway.
- New goal params: `settle_tolerance_m` (default 5 mm) and
  `settle_timeout_ms` (default 1500 ms). Drops the old `delay` param,
  which existed only because nothing else governed the cadence.

Changes to `robot.ex`:

- Bump every joint's `acceleration` limit from `720 °/s²` to `2160 °/s²`.
  This is consumed only by `BB.Sim.Actuator` when building its
  trapezoidal motion profile, so it speeds up the visual sim without
  affecting real-hardware behaviour (the Feetech STS3215 uses its own
  internal velocity profile). Halves the per-waypoint motion time.

End-to-end the sim demo now completes in ~8 s (previously ~25 s with
`wait_for_arrival` at the original limits, or producing a non-circular
trace at the original 150 ms cadence) and the EE actually visits every
commanded point.
@jimsynz jimsynz merged commit 292cb8d into main May 14, 2026
25 of 29 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant